// Copyright (C) 2004, 2011 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Eclipse Public License.
//
// Authors:  Carl Laird, Andreas Waechter     IBM    2004-08-13

#include "IpOrigIpoptNLP.hpp"
#include "IpLowRankUpdateSymMatrix.hpp"
#include "IpIpoptData.hpp"
#include "IpIpoptCalculatedQuantities.hpp"

#include <cstdio>
#include <cassert>
#include <cmath>

namespace Ipopt
{
#if IPOPT_VERBOSITY > 0
static const Index dbg_verbosity = 0;
#endif

OrigIpoptNLP::OrigIpoptNLP(
   const SmartPtr<const Journalist>& jnlst,
   const SmartPtr<NLP>&              nlp,
   const SmartPtr<NLPScalingObject>& nlp_scaling,
   TimingStatistics&                 timing_statistics
)
   : IpoptNLP(nlp_scaling),
     jnlst_(jnlst),
     nlp_(nlp),
     x_space_(NULL),
     f_cache_(1),
     grad_f_cache_(1),
     c_cache_(1),
     jac_c_cache_(1),
     d_cache_(1),
     jac_d_cache_(1),
     h_cache_(1),
     unscaled_x_cache_(1),
     initialized_(false),
     timing_statistics_(timing_statistics)
{
}

OrigIpoptNLP::~OrigIpoptNLP()
{
}

void OrigIpoptNLP::RegisterOptions(
   SmartPtr<RegisteredOptions> roptions
)
{
   roptions->AddLowerBoundedNumberOption(
      "bound_relax_factor",
      "Factor for initial relaxation of the bounds.",
      0., false,
      1e-8,
      "Before start of the optimization, the bounds given by the user are relaxed. "
      "This option sets the factor for this relaxation. "
      "Additional, the constraint violation tolerance constr_viol_tol is used to bound the relaxation by an absolute value. "
      "If it is set to zero, then then bounds relaxation is disabled. "
      "See Eqn.(35) in implementation paper. "
      "Note that the constraint violation reported by Ipopt at the end of the solution process "
      "does not include violations of the original (non-relaxed) variable bounds. See also option honor_original_bounds.");
   roptions->AddBoolOption(
      "honor_original_bounds",
      "Indicates whether final points should be projected into original bounds.",
      false,
      "Ipopt might relax the bounds during the optimization (see, e.g., option \"bound_relax_factor\"). "
      "This option determines whether the final point should be projected back into the user-provide original bounds after the optimization. "
      "Note that violations of constraints and complementarity reported by Ipopt at the end of the solution process "
      "are for the non-projected point.");
   roptions->SetRegisteringCategory("Warm Start");
   roptions->AddBoolOption(
      "warm_start_same_structure",
      "Indicates whether a problem with a structure identical to the previous one is to be solved.",
      false,
      "If enabled, then the algorithm assumes that an NLP is now to be solved "
      "whose structure is identical to one that already was considered (with the same NLP object).",
      true);
   roptions->SetRegisteringCategory("NLP");
   roptions->AddBoolOption(
      "check_derivatives_for_naninf",
      "Indicates whether it is desired to check for Nan/Inf in derivative matrices",
      false,
      "Activating this option will cause an error if an invalid number is detected "
      "in the constraint Jacobians or the Lagrangian Hessian. "
      "If this is not activated, the test is skipped, and the algorithm might proceed with invalid numbers and fail. "
      "If test is activated and an invalid number is detected, "
      "the matrix is written to output with print_level corresponding to J_MOREDETAILED (7); "
      "so beware of large output!");
   roptions->AddBoolOption(
      "grad_f_constant",
      "Indicates whether to assume that the objective function is linear",
      false,
      "Activating this option will cause Ipopt to ask for the Gradient of the objective function "
      "only once from the NLP and reuse this information later.");
   roptions->AddBoolOption(
      "jac_c_constant",
      "Indicates whether to assume that all equality constraints are linear",
      false,
      "Activating this option will cause Ipopt to ask for the Jacobian of the equality constraints "
      "only once from the NLP and reuse this information later.");
   roptions->AddBoolOption(
      "jac_d_constant",
      "Indicates whether to assume that all inequality constraints are linear",
      false,
      "Activating this option will cause Ipopt to ask for the Jacobian of the inequality constraints "
      "only once from the NLP and reuse this information later.");
   roptions->AddBoolOption(
      "hessian_constant",
      "Indicates whether to assume the problem is a QP (quadratic objective, linear constraints)",
      false,
      "Activating this option will cause Ipopt to ask for the Hessian of the Lagrangian function "
      "only once from the NLP and reuse this information later.");
   roptions->SetRegisteringCategory("Hessian Approximation");
   roptions->AddStringOption2(
      "hessian_approximation",
      "Indicates what Hessian information is to be used.",
      "exact",
      "exact", "Use second derivatives provided by the NLP.",
      "limited-memory", "Perform a limited-memory quasi-Newton approximation",
      "This determines which kind of information for the Hessian of the Lagrangian function is used by the algorithm.");
   roptions->AddStringOption2(
      "hessian_approximation_space",
      "Indicates in which subspace the Hessian information is to be approximated.",
      "nonlinear-variables",
      "nonlinear-variables", "only in space of nonlinear variables.",
      "all-variables", "in space of all variables (without slacks)",
      "",
      true);
}

bool OrigIpoptNLP::Initialize(
   const Journalist&  jnlst,
   const OptionsList& options,
   const std::string& prefix
)
{
   options.GetNumericValue("bound_relax_factor", bound_relax_factor_, prefix);
   options.GetNumericValue("constr_viol_tol", constr_viol_tol_, prefix);
   options.GetBoolValue("honor_original_bounds", honor_original_bounds_, prefix);
   options.GetBoolValue("warm_start_same_structure", warm_start_same_structure_, prefix);
   options.GetBoolValue("check_derivatives_for_naninf", check_derivatives_for_naninf_, prefix);
   Index enum_int;
   options.GetEnumValue("hessian_approximation", enum_int, prefix);
   hessian_approximation_ = HessianApproximationType(enum_int);
   options.GetEnumValue("hessian_approximation_space", enum_int, prefix);
   hessian_approximation_space_ = HessianApproximationSpace(enum_int);

   options.GetBoolValue("grad_f_constant", grad_f_constant_, prefix);
   options.GetBoolValue("jac_c_constant", jac_c_constant_, prefix);
   options.GetBoolValue("jac_d_constant", jac_d_constant_, prefix);
   options.GetBoolValue("hessian_constant", hessian_constant_, prefix);

   // Reset the function evaluation counters (for warm start)
   f_evals_ = 0;
   grad_f_evals_ = 0;
   c_evals_ = 0;
   jac_c_evals_ = 0;
   d_evals_ = 0;
   jac_d_evals_ = 0;
   h_evals_ = 0;

   if( !warm_start_same_structure_ )
   {
      // Reset all caches.
      grad_f_cache_.Clear();
      c_cache_.Clear();
      jac_c_cache_.Clear();
      d_cache_.Clear();
      jac_d_cache_.Clear();
      // If the hessian is constant, we want two hessians to be
      // cached, one for regular iterations and one for restoration
      // phase
      if( hessian_constant_ )
      {
         h_cache_.Clear(2);
      }
      else
      {
         h_cache_.Clear(1);
      }
   }

   // Reset the cache entries belonging to a dummy dependency.  This
   // is required for repeated solve, since the cache is not updated
   // if a dimension is zero.  It is also required if we choose
   // jac_[cd]_constant, grad_f_constant, or hessian_constant differently between
   // runs or when the values of these derivatives changes due to a problem change.
   std::vector<const TaggedObject*> deps(1);
   deps[0] = NULL;
   std::vector<Number> sdeps(0);
   grad_f_cache_.InvalidateResult(deps, sdeps);
   c_cache_.InvalidateResult(deps, sdeps);
   d_cache_.InvalidateResult(deps, sdeps);
   jac_c_cache_.InvalidateResult(deps, sdeps);
   jac_d_cache_.InvalidateResult(deps, sdeps);
   deps.resize(3, NULL);
   h_cache_.InvalidateResult(deps, sdeps);

   if( !nlp_->ProcessOptions(options, prefix) )
   {
      return false;
   }

   initialized_ = true;
   return IpoptNLP::Initialize(jnlst, options, prefix);
}

bool OrigIpoptNLP::InitializeStructures(
   SmartPtr<Vector>& x,
   bool              init_x,
   SmartPtr<Vector>& y_c,
   bool              init_y_c,
   SmartPtr<Vector>& y_d,
   bool              init_y_d,
   SmartPtr<Vector>& z_L,
   bool              init_z_L,
   SmartPtr<Vector>& z_U,
   bool              init_z_U,
   SmartPtr<Vector>& v_L,
   SmartPtr<Vector>& v_U
)
{
   DBG_START_METH("OrigIpoptNLP::InitializeStructures", dbg_verbosity);
   DBG_ASSERT(initialized_);
   bool retValue;

   SmartPtr<Vector> x_L;
   SmartPtr<Matrix> Px_L;
   SmartPtr<Vector> x_U;
   SmartPtr<Matrix> Px_U;
   SmartPtr<Vector> d_L;
   SmartPtr<Matrix> Pd_L;
   SmartPtr<Vector> d_U;
   SmartPtr<Matrix> Pd_U;

   if( !warm_start_same_structure_ )
   {

      retValue = nlp_->GetSpaces(x_space_, c_space_, d_space_, x_l_space_, px_l_space_, x_u_space_, px_u_space_,
                                 d_l_space_, pd_l_space_, d_u_space_, pd_u_space_, jac_c_space_, jac_d_space_, h_space_);

      if( !retValue )
      {
         jnlst_->Printf(J_WARNING, J_INITIALIZATION, "GetSpaces method for the NLP returns false.\n");
         return false;
      }

      // Check if the Hessian space is actually a limited-memory
      // approximation.  If so, get the required information from the
      // NLP and create an appropreate h_space
      if( hessian_approximation_ == LIMITED_MEMORY )
      {
         SmartPtr<VectorSpace> approx_vecspace;
         SmartPtr<Matrix> P_approx;
         if( hessian_approximation_space_ == NONLINEAR_VARS )
         {
            nlp_->GetQuasiNewtonApproximationSpaces(approx_vecspace, P_approx);
         }
         if( IsValid(approx_vecspace) )
         {
            DBG_ASSERT(IsValid(P_approx));
            h_space_ = new LowRankUpdateSymMatrixSpace(x_space_->Dim(), ConstPtr(P_approx), ConstPtr(approx_vecspace),
                  true);
            jnlst_->Printf(J_DETAILED, J_INITIALIZATION,
                           "Hessian approximation will be done in smaller space of dimension %" IPOPT_INDEX_FORMAT " (instead of %" IPOPT_INDEX_FORMAT ")\n\n",
                           P_approx->NCols(), P_approx->NRows());
         }
         else
         {
            DBG_ASSERT(IsNull(P_approx));
            h_space_ = new LowRankUpdateSymMatrixSpace(x_space_->Dim(), ConstPtr(P_approx), ConstPtr(x_space_), true);
            jnlst_->Printf(J_DETAILED, J_INITIALIZATION,
                           "Hessian approximation will be done in the space of all %" IPOPT_INDEX_FORMAT " x variables.\n\n", x_space_->Dim());
         }
      }

      // Create the bounds structures
      x_L = x_l_space_->MakeNew();
      Px_L = px_l_space_->MakeNew();
      x_U = x_u_space_->MakeNew();
      Px_U = px_u_space_->MakeNew();
      d_L = d_l_space_->MakeNew();
      Pd_L = pd_l_space_->MakeNew();
      d_U = d_u_space_->MakeNew();
      Pd_U = pd_u_space_->MakeNew();

      retValue = nlp_->GetBoundsInformation(*Px_L, *x_L, *Px_U, *x_U, *Pd_L, *d_L, *Pd_U, *d_U);
      if( !retValue )
      {
         return false;
      }

      NLP_scaling()->DetermineScaling(x_space_, c_space_, d_space_, jac_c_space_, jac_d_space_, h_space_,
                                      scaled_jac_c_space_, scaled_jac_d_space_, scaled_h_space_, *Px_L, *x_L, *Px_U, *x_U);

      if( x_space_->Dim() < c_space_->Dim() && x_space_->Dim() > 0 )
      {
         char msg[128];
         Snprintf(msg, 127, "Too few degrees of freedom: %" IPOPT_INDEX_FORMAT " equality constraints but only %" IPOPT_INDEX_FORMAT " variables",
                  c_space_->Dim(), x_space_->Dim());
         THROW_EXCEPTION(TOO_FEW_DOF, msg);
      }

      // cannot have any null pointers, want zero length vectors
      // instead of null - this will later need to be changed for _h;
      DBG_DO(retValue = IsValid(x_space_) && IsValid(c_space_) && IsValid(d_space_) && IsValid(x_l_space_)
                        && IsValid(px_l_space_) && IsValid(x_u_space_) && IsValid(px_u_space_) && IsValid(d_u_space_)
                        && IsValid(pd_u_space_) && IsValid(d_l_space_) && IsValid(pd_l_space_) && IsValid(jac_c_space_)
                        && IsValid(jac_d_space_) && IsValid(h_space_) && IsValid(scaled_jac_c_space_) && IsValid(scaled_jac_d_space_)
                        && IsValid(scaled_h_space_));

      DBG_ASSERT(retValue && "Model cannot return null vector or matrix prototypes or spaces,"
                 " please return zero length vectors instead");
   }
   else
   {
      ASSERT_EXCEPTION(IsValid(x_space_), INVALID_WARMSTART,
                       "OrigIpoptNLP called with warm_start_same_structure, but the problem is solved for the first time.");
      // Create the bounds structures
      x_L = x_l_space_->MakeNew();
      Px_L = px_l_space_->MakeNew();
      x_U = x_u_space_->MakeNew();
      Px_U = px_u_space_->MakeNew();
      d_L = d_l_space_->MakeNew();
      Pd_L = pd_l_space_->MakeNew();
      d_U = d_u_space_->MakeNew();
      Pd_U = pd_u_space_->MakeNew();

      retValue = nlp_->GetBoundsInformation(*Px_L, *x_L, *Px_U, *x_U, *Pd_L, *d_L, *Pd_U, *d_U);
      if( !retValue )
      {
         return false;
      }
   }

   x_L->Print(*jnlst_, J_MOREVECTOR, J_INITIALIZATION, "original x_L unscaled");
   x_U->Print(*jnlst_, J_MOREVECTOR, J_INITIALIZATION, "original x_U unscaled");
   d_L->Print(*jnlst_, J_MOREVECTOR, J_INITIALIZATION, "original d_L unscaled");
   d_U->Print(*jnlst_, J_MOREVECTOR, J_INITIALIZATION, "original d_U unscaled");

   if( bound_relax_factor_ > 0.0 )
   {
      SmartPtr<Vector> tmp;
      tmp = x_L->MakeNewCopy();
      orig_x_L_ = ConstPtr(tmp);
      tmp = x_U->MakeNewCopy();
      orig_x_U_ = ConstPtr(tmp);
      tmp = d_L->MakeNewCopy();
      orig_d_L_ = ConstPtr(tmp);
      tmp = d_U->MakeNewCopy();
      orig_d_U_ = ConstPtr(tmp);

      relax_bounds(-bound_relax_factor_, *x_L);
      relax_bounds(bound_relax_factor_, *x_U);
      relax_bounds(-bound_relax_factor_, *d_L);
      relax_bounds(bound_relax_factor_, *d_U);
   }

   x_L_ = ConstPtr(x_L);
   Px_L_ = ConstPtr(Px_L);
   x_U_ = ConstPtr(x_U);
   Px_U_ = ConstPtr(Px_U);
   d_L_ = ConstPtr(d_L);
   Pd_L_ = ConstPtr(Pd_L);
   d_U_ = ConstPtr(d_U);
   Pd_U_ = ConstPtr(Pd_U);

   // now create and store the scaled bounds
   x_L_ = NLP_scaling()->apply_vector_scaling_x_LU(*Px_L_, x_L_, *x_space_);
   x_U_ = NLP_scaling()->apply_vector_scaling_x_LU(*Px_U_, x_U_, *x_space_);
   d_L_ = NLP_scaling()->apply_vector_scaling_d_LU(*Pd_L_, d_L_, *d_space_);
   d_U_ = NLP_scaling()->apply_vector_scaling_d_LU(*Pd_U_, d_U_, *d_space_);

   x_L_->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "modified x_L scaled");
   x_U_->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "modified x_U scaled");
   d_L_->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "modified d_L scaled");
   d_U_->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "modified d_U scaled");

   // Create the iterates structures
   x = x_space_->MakeNew();
   y_c = c_space_->MakeNew();
   y_d = d_space_->MakeNew();
   z_L = x_l_space_->MakeNew();
   z_U = x_u_space_->MakeNew();
   v_L = d_l_space_->MakeNew();
   v_U = d_u_space_->MakeNew();

   retValue = nlp_->GetStartingPoint(GetRawPtr(x), init_x, GetRawPtr(y_c), init_y_c, GetRawPtr(y_d), init_y_d,
                                     GetRawPtr(z_L), init_z_L, GetRawPtr(z_U), init_z_U);

   if( !retValue )
   {
      return false;
   }

   Number obj_scal = NLP_scaling()->apply_obj_scaling(1.);
   if( init_x )
   {
      x->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "initial x unscaled");
      if( NLP_scaling()->have_x_scaling() )
      {
         x = NLP_scaling()->apply_vector_scaling_x_NonConst(ConstPtr(x));
      }
   }
   if( init_y_c )
   {
      y_c->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "initial y_c unscaled");
      if( NLP_scaling()->have_c_scaling() )
      {
         y_c = NLP_scaling()->unapply_vector_scaling_c_NonConst(ConstPtr(y_c));
      }
      if( obj_scal != 1. )
      {
         y_c->Scal(obj_scal);
      }
   }
   if( init_y_d )
   {
      y_d->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "initial y_d unscaled");
      if( NLP_scaling()->have_d_scaling() )
      {
         y_d = NLP_scaling()->unapply_vector_scaling_d_NonConst(ConstPtr(y_d));
      }
      if( obj_scal != 1. )
      {
         y_d->Scal(obj_scal);
      }
   }
   if( init_z_L )
   {
      z_L->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "initial z_L unscaled");
      if( NLP_scaling()->have_x_scaling() )
      {
         z_L = NLP_scaling()->apply_vector_scaling_x_LU_NonConst(*Px_L_, ConstPtr(z_L), *x_space_);
      }
      if( obj_scal != 1. )
      {
         z_L->Scal(obj_scal);
      }
   }
   if( init_z_U )
   {
      z_U->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "initial z_U unscaled");
      if( NLP_scaling()->have_x_scaling() )
      {
         z_U = NLP_scaling()->apply_vector_scaling_x_LU_NonConst(*Px_U_, ConstPtr(z_U), *x_space_);
      }
      if( obj_scal != 1. )
      {
         z_U->Scal(obj_scal);
      }
   }

   return true;
}

void OrigIpoptNLP::relax_bounds(
   Number  bound_relax_factor,
   Vector& bounds
)
{
   DBG_START_METH("OrigIpoptNLP::relax_bounds", dbg_verbosity);
   DBG_ASSERT(bound_relax_factor != 0.);  // checked in caller

   SmartPtr<Vector> tmp = bounds.MakeNew();
   tmp->Copy(bounds);
   tmp->ElementWiseAbs();
   tmp->Scal(std::abs(bound_relax_factor)); // |relaxfactor|*|bounds|
   SmartPtr<Vector> ones = bounds.MakeNew();
   ones->Set(std::abs(bound_relax_factor));
   tmp->ElementWiseMax(*ones);    // |relaxfactor|*max(|bounds|,1)
   ones->Set(constr_viol_tol_);
   tmp->ElementWiseMin(*ones);    // min(constr_viol_tol_,|relaxfactor|*max(|bounds|,1))
   DBG_PRINT((1, "bound_relax_factor = %e constr_viol_tol = %e", bound_relax_factor, constr_viol_tol_));
   DBG_PRINT_VECTOR(2, "tmp", *tmp);
   DBG_PRINT_VECTOR(2, "bounds before", bounds);
   bounds.Axpy(bound_relax_factor < 0.0 ? -1.0 : 1.0, *tmp);
   DBG_PRINT_VECTOR(2, "bounds after", bounds);
}

Number OrigIpoptNLP::f(
   const Vector& x
)
{
   DBG_START_METH("OrigIpoptNLP::f", dbg_verbosity);
   Number ret = 0.0;
   DBG_PRINT((2, "x.Tag = %u\n", x.GetTag()));
   if( !f_cache_.GetCachedResult1Dep(ret, &x) )
   {
      f_evals_++;
      SmartPtr<const Vector> unscaled_x = get_unscaled_x(x);
      timing_statistics_.f_eval_time().Start();
      bool success = nlp_->Eval_f(*unscaled_x, ret);
      timing_statistics_.f_eval_time().End();
      DBG_PRINT((1, "success = %d ret = %e\n", success, ret));
      ASSERT_EXCEPTION(success && IsFiniteNumber(ret), Eval_Error, "Error evaluating the objective function");
      ret = NLP_scaling()->apply_obj_scaling(ret);
      f_cache_.AddCachedResult1Dep(ret, &x);
   }

   return ret;
}

Number OrigIpoptNLP::f(
   const Vector& /*x*/,
   Number        /*mu*/
)
{
   assert(false && "ERROR: This method is only a placeholder for f(mu) and should not be called");
   return 0.;
}

SmartPtr<const Vector> OrigIpoptNLP::grad_f(
   const Vector& x
)
{
   SmartPtr<Vector> unscaled_grad_f;
   SmartPtr<const Vector> retValue;
   const Vector* dep = NULL;
   if( !grad_f_constant_ )
   {
      dep = &x;
   }
   if( !grad_f_cache_.GetCachedResult1Dep(retValue, dep) )
   {
      grad_f_evals_++;
      unscaled_grad_f = x_space_->MakeNew();

      SmartPtr<const Vector> unscaled_x = get_unscaled_x(x);
      timing_statistics_.grad_f_eval_time().Start();
      bool success = nlp_->Eval_grad_f(*unscaled_x, *unscaled_grad_f);
      timing_statistics_.grad_f_eval_time().End();
      ASSERT_EXCEPTION(success && IsFiniteNumber(unscaled_grad_f->Nrm2()), Eval_Error,
                       "Error evaluating the gradient of the objective function");
      retValue = NLP_scaling()->apply_grad_obj_scaling(ConstPtr(unscaled_grad_f));
      grad_f_cache_.AddCachedResult1Dep(retValue, dep);
   }

   return retValue;
}

SmartPtr<const Vector> OrigIpoptNLP::grad_f(
   const Vector& /*x*/,
   Number /*mu*/
)
{
   THROW_EXCEPTION(INTERNAL_ABORT, "ERROR: This method is only a placeholder for grad_f(mu) and should not be called");
   return NULL;
}

SmartPtr<const Vector> OrigIpoptNLP::c(
   const Vector& x
)
{
   SmartPtr<const Vector> retValue;
   if( c_space_->Dim() == 0 )
   {
      // We do this caching of an empty vector so that the returned
      // Vector has always the same tag (this might make a difference
      // in cases where only the constraints are supposed to change...
      SmartPtr<const Vector> dep = NULL;
      if( !c_cache_.GetCachedResult1Dep(retValue, GetRawPtr(dep)) )
      {
         retValue = c_space_->MakeNew();
         c_cache_.AddCachedResult1Dep(retValue, GetRawPtr(dep));
      }
   }
   else
   {
      if( !c_cache_.GetCachedResult1Dep(retValue, x) )
      {
         SmartPtr<Vector> unscaled_c = c_space_->MakeNew();
         c_evals_++;
         SmartPtr<const Vector> unscaled_x = get_unscaled_x(x);
         timing_statistics_.c_eval_time().Start();
         bool success = nlp_->Eval_c(*unscaled_x, *unscaled_c);
         timing_statistics_.c_eval_time().End();
         if( !success || !IsFiniteNumber(unscaled_c->Nrm2()) )
         {
            if( check_derivatives_for_naninf_ && !IsFiniteNumber(unscaled_c->Nrm2()) )
            {
               jnlst_->Printf(J_WARNING, J_NLP, "The equality constraints contain an invalid number\n");
               unscaled_c->Print(*jnlst_, J_MOREDETAILED, J_MAIN, "unscaled_c");
               jnlst_->FlushBuffer();
            }
            THROW_EXCEPTION(Eval_Error, "Error evaluating the equality constraints");
         }
         retValue = NLP_scaling()->apply_vector_scaling_c(ConstPtr(unscaled_c));
         c_cache_.AddCachedResult1Dep(retValue, x);
      }
   }

   return retValue;
}

SmartPtr<const Vector> OrigIpoptNLP::d(
   const Vector& x
)
{
   DBG_START_METH("OrigIpoptNLP::d", dbg_verbosity);
   SmartPtr<const Vector> retValue;
   if( d_space_->Dim() == 0 )
   {
      // We do this caching of an empty vector so that the returned
      // Vector has always the same tag (this might make a difference
      // in cases where only the constraints are supposed to change...
      SmartPtr<const Vector> dep = NULL;
      if( !d_cache_.GetCachedResult1Dep(retValue, GetRawPtr(dep)) )
      {
         retValue = d_space_->MakeNew();
         d_cache_.AddCachedResult1Dep(retValue, GetRawPtr(dep));
      }
   }
   else
   {
      if( !d_cache_.GetCachedResult1Dep(retValue, x) )
      {
         d_evals_++;
         SmartPtr<Vector> unscaled_d = d_space_->MakeNew();

         DBG_PRINT_VECTOR(2, "scaled_x", x);
         SmartPtr<const Vector> unscaled_x = get_unscaled_x(x);
         timing_statistics_.d_eval_time().Start();
         bool success = nlp_->Eval_d(*unscaled_x, *unscaled_d);
         timing_statistics_.d_eval_time().End();
         DBG_PRINT_VECTOR(2, "unscaled_d", *unscaled_d);
         if( !success || !IsFiniteNumber(unscaled_d->Nrm2()) )
         {
            if( check_derivatives_for_naninf_ && !IsFiniteNumber(unscaled_d->Nrm2()) )
            {
               jnlst_->Printf(J_WARNING, J_NLP, "The inequality constraints contain an invalid number\n");
               unscaled_d->Print(*jnlst_, J_MOREDETAILED, J_MAIN, "unscaled_d");
               jnlst_->FlushBuffer();
            }
            THROW_EXCEPTION(Eval_Error, "Error evaluating the inequality constraints");
         }
         retValue = NLP_scaling()->apply_vector_scaling_d(ConstPtr(unscaled_d));
         d_cache_.AddCachedResult1Dep(retValue, x);
      }
   }

   return retValue;
}

SmartPtr<const Matrix> OrigIpoptNLP::jac_c(
   const Vector& x
)
{
   SmartPtr<const Matrix> retValue;
   if( c_space_->Dim() == 0 )
   {
      // We do this caching of an empty vector so that the returned
      // Matrix has always the same tag
      SmartPtr<const Vector> dep = NULL;
      if( !jac_c_cache_.GetCachedResult1Dep(retValue, GetRawPtr(dep)) )
      {
         SmartPtr<Matrix> unscaled_jac_c = jac_c_space_->MakeNew();
         retValue = NLP_scaling()->apply_jac_c_scaling(ConstPtr(unscaled_jac_c));
         jac_c_cache_.AddCachedResult1Dep(retValue, GetRawPtr(dep));
      }
   }
   else
   {
      SmartPtr<const Vector> dep = NULL;
      if( !jac_c_constant_ )
      {
         dep = &x;
      }
      if( !jac_c_cache_.GetCachedResult1Dep(retValue, GetRawPtr(dep)) )
      {
         jac_c_evals_++;
         SmartPtr<Matrix> unscaled_jac_c = jac_c_space_->MakeNew();

         SmartPtr<const Vector> unscaled_x = get_unscaled_x(x);
         timing_statistics_.jac_c_eval_time().Start();
         bool success = nlp_->Eval_jac_c(*unscaled_x, *unscaled_jac_c);
         timing_statistics_.jac_c_eval_time().End();
         ASSERT_EXCEPTION(success, Eval_Error, "Error evaluating the jacobian of the equality constraints");
         if( check_derivatives_for_naninf_ )
         {
            if( !unscaled_jac_c->HasValidNumbers() )
            {
               jnlst_->Printf(J_WARNING, J_NLP,
                              "The Jacobian for the equality constraints contains an invalid number\n");
               unscaled_jac_c->Print(*jnlst_, J_MOREDETAILED, J_MAIN, "unscaled_jac_c");
               jnlst_->FlushBuffer();
               THROW_EXCEPTION(Eval_Error, "The Jacobian for the equality constraints contains an invalid number");
            }
         }
         retValue = NLP_scaling()->apply_jac_c_scaling(ConstPtr(unscaled_jac_c));
         jac_c_cache_.AddCachedResult1Dep(retValue, GetRawPtr(dep));
      }
   }

   return retValue;
}

SmartPtr<const Matrix> OrigIpoptNLP::jac_d(
   const Vector& x
)
{
   SmartPtr<const Matrix> retValue;
   if( d_space_->Dim() == 0 )
   {
      // We do this caching of an empty vector so that the returned
      // Matrix has always the same tag
      SmartPtr<const Vector> dep = NULL;
      if( !jac_d_cache_.GetCachedResult1Dep(retValue, GetRawPtr(dep)) )
      {
         SmartPtr<Matrix> unscaled_jac_d = jac_d_space_->MakeNew();
         retValue = NLP_scaling()->apply_jac_d_scaling(ConstPtr(unscaled_jac_d));
         jac_d_cache_.AddCachedResult1Dep(retValue, GetRawPtr(dep));
      }
   }
   else
   {
      SmartPtr<const Vector> dep = NULL;
      if( !jac_d_constant_ )
      {
         dep = &x;
      }

      if( !jac_d_cache_.GetCachedResult1Dep(retValue, GetRawPtr(dep)) )
      {
         jac_d_evals_++;
         SmartPtr<Matrix> unscaled_jac_d = jac_d_space_->MakeNew();

         SmartPtr<const Vector> unscaled_x = get_unscaled_x(x);
         timing_statistics_.jac_d_eval_time().Start();
         bool success = nlp_->Eval_jac_d(*unscaled_x, *unscaled_jac_d);
         timing_statistics_.jac_d_eval_time().End();
         ASSERT_EXCEPTION(success, Eval_Error, "Error evaluating the jacobian of the inequality constraints");
         if( check_derivatives_for_naninf_ )
         {
            if( !unscaled_jac_d->HasValidNumbers() )
            {
               jnlst_->Printf(J_WARNING, J_NLP,
                              "The Jacobian for the inequality constraints contains an invalid number\n");

               unscaled_jac_d->Print(*jnlst_, J_MOREDETAILED, J_MAIN, "unscaled_jac_d");
               jnlst_->FlushBuffer();
               THROW_EXCEPTION(Eval_Error, "The Jacobian for the inequality constraints contains an invalid number");
            }
         }
         retValue = NLP_scaling()->apply_jac_d_scaling(ConstPtr(unscaled_jac_d));
         jac_d_cache_.AddCachedResult1Dep(retValue, GetRawPtr(dep));
      }
   }

   return retValue;
}

SmartPtr<const SymMatrix> OrigIpoptNLP::uninitialized_h()
{
   return h_space_->MakeNewSymMatrix();
}

SmartPtr<const SymMatrix> OrigIpoptNLP::h(
   const Vector& x,
   Number        obj_factor,
   const Vector& yc,
   const Vector& yd
)
{
   SmartPtr<SymMatrix> unscaled_h;
   SmartPtr<const SymMatrix> retValue;

   std::vector<const TaggedObject*> deps(3);
   if( !hessian_constant_ )
   {
      deps[0] = &x;
      deps[1] = &yc;
      deps[2] = &yd;
   }
   else
   {
      deps[0] = NULL;
      deps[1] = NULL;
      deps[2] = NULL;
   }
   std::vector<Number> scalar_deps(1);
   scalar_deps[0] = obj_factor;

   if( !h_cache_.GetCachedResult(retValue, deps, scalar_deps) )
   {
      h_evals_++;
      unscaled_h = h_space_->MakeNewSymMatrix();

      SmartPtr<const Vector> unscaled_x = get_unscaled_x(x);
      SmartPtr<const Vector> unscaled_yc = NLP_scaling()->apply_vector_scaling_c(&yc);
      SmartPtr<const Vector> unscaled_yd = NLP_scaling()->apply_vector_scaling_d(&yd);
      Number scaled_obj_factor = NLP_scaling()->apply_obj_scaling(obj_factor);
      timing_statistics_.h_eval_time().Start();
      bool success = nlp_->Eval_h(*unscaled_x, scaled_obj_factor, *unscaled_yc, *unscaled_yd, *unscaled_h);
      timing_statistics_.h_eval_time().End();
      ASSERT_EXCEPTION(success, Eval_Error, "Error evaluating the hessian of the lagrangian");
      if( check_derivatives_for_naninf_ )
      {
         if( !unscaled_h->HasValidNumbers() )
         {
            jnlst_->Printf(J_WARNING, J_NLP, "The Lagrangian Hessian contains an invalid number\n");

            unscaled_h->Print(*jnlst_, J_MOREDETAILED, J_MAIN, "unscaled_h");
            jnlst_->FlushBuffer();
            THROW_EXCEPTION(Eval_Error, "The Lagrangian Hessian contains an invalid number");
         }
      }
      retValue = NLP_scaling()->apply_hessian_scaling(ConstPtr(unscaled_h));
      h_cache_.AddCachedResult(retValue, deps, scalar_deps);
   }

   return retValue;
}

SmartPtr<const SymMatrix> OrigIpoptNLP::h(
   const Vector& /*x*/,
   Number        /*obj_factor*/,
   const Vector& /*yc*/,
   const Vector& /*yd*/,
   Number        /*mu*/
)
{
   THROW_EXCEPTION(INTERNAL_ABORT, "ERROR: This method is only a for h(mu) and should not be called");
   return NULL;
}

void OrigIpoptNLP::GetSpaces(
   SmartPtr<const VectorSpace>&    x_space,
   SmartPtr<const VectorSpace>&    c_space,
   SmartPtr<const VectorSpace>&    d_space,
   SmartPtr<const VectorSpace>&    x_l_space,
   SmartPtr<const MatrixSpace>&    px_l_space,
   SmartPtr<const VectorSpace>&    x_u_space,
   SmartPtr<const MatrixSpace>&    px_u_space,
   SmartPtr<const VectorSpace>&    d_l_space,
   SmartPtr<const MatrixSpace>&    pd_l_space,
   SmartPtr<const VectorSpace>&    d_u_space,
   SmartPtr<const MatrixSpace>&    pd_u_space,
   SmartPtr<const MatrixSpace>&    Jac_c_space,
   SmartPtr<const MatrixSpace>&    Jac_d_space,
   SmartPtr<const SymMatrixSpace>& Hess_lagrangian_space
)
{
   // Make sure that we already have all the pointers
   DBG_ASSERT(IsValid(x_space_) &&
              IsValid(c_space_) &&
              IsValid(d_space_) &&
              IsValid(x_l_space_) &&
              IsValid(px_l_space_) &&
              IsValid(x_u_space_) &&
              IsValid(px_u_space_) &&
              IsValid(d_l_space_) &&
              IsValid(pd_l_space_) &&
              IsValid(d_u_space_) &&
              IsValid(pd_u_space_) &&
              IsValid(scaled_jac_c_space_) &&
              IsValid(scaled_jac_d_space_) &&
              IsValid(scaled_h_space_));

   DBG_ASSERT(IsValid(NLP_scaling()));

   x_space = x_space_;
   c_space = c_space_;
   d_space = d_space_;
   x_l_space = x_l_space_;
   px_l_space = px_l_space_;
   x_u_space = x_u_space_;
   px_u_space = px_u_space_;
   d_l_space = d_l_space_;
   pd_l_space = pd_l_space_;
   d_u_space = d_u_space_;
   pd_u_space = pd_u_space_;
   Jac_c_space = scaled_jac_c_space_;
   Jac_d_space = scaled_jac_d_space_;
   Hess_lagrangian_space = scaled_h_space_;
}

void OrigIpoptNLP::FinalizeSolution(
   SolverReturn               status,
   const Vector&              x,
   const Vector&              z_L,
   const Vector&              z_U,
   const Vector&              c,
   const Vector&              d,
   const Vector&              y_c,
   const Vector&              y_d,
   Number                     obj_value,
   const IpoptData*           ip_data,
   IpoptCalculatedQuantities* ip_cq
)
{
   DBG_START_METH("OrigIpoptNLP::FinalizeSolution", dbg_verbosity);
   // need to submit the unscaled solution back to the nlp
   SmartPtr<const Vector> unscaled_x = get_unscaled_x(x);
   SmartPtr<const Vector> unscaled_c = NLP_scaling()->unapply_vector_scaling_c(&c);
   SmartPtr<const Vector> unscaled_d = NLP_scaling()->unapply_vector_scaling_d(&d);
   const Number unscaled_obj = NLP_scaling()->unapply_obj_scaling(obj_value);

   SmartPtr<const Vector> unscaled_z_L;
   SmartPtr<const Vector> unscaled_z_U;
   SmartPtr<const Vector> unscaled_y_c;
   SmartPtr<const Vector> unscaled_y_d;

   // The objective function scaling factor also appears in the constraints
   Number obj_unscale_factor = NLP_scaling()->unapply_obj_scaling(1.);
   if( obj_unscale_factor != 1. )
   {
      SmartPtr<Vector> tmp = NLP_scaling()->apply_vector_scaling_x_LU_NonConst(*Px_L_, &z_L, *x_space_);
      tmp->Scal(obj_unscale_factor);
      unscaled_z_L = ConstPtr(tmp);

      tmp = NLP_scaling()->apply_vector_scaling_x_LU_NonConst(*Px_U_, &z_U, *x_space_);
      tmp->Scal(obj_unscale_factor);
      unscaled_z_U = ConstPtr(tmp);

      tmp = NLP_scaling()->apply_vector_scaling_c_NonConst(&y_c);
      tmp->Scal(obj_unscale_factor);
      unscaled_y_c = ConstPtr(tmp);

      tmp = NLP_scaling()->apply_vector_scaling_d_NonConst(&y_d);
      tmp->Scal(obj_unscale_factor);
      unscaled_y_d = ConstPtr(tmp);
   }
   else
   {
      unscaled_z_L = NLP_scaling()->apply_vector_scaling_x_LU(*Px_L_, &z_L, *x_space_);
      unscaled_z_U = NLP_scaling()->apply_vector_scaling_x_LU(*Px_U_, &z_U, *x_space_);
      unscaled_y_c = NLP_scaling()->apply_vector_scaling_c(&y_c);
      unscaled_y_d = NLP_scaling()->apply_vector_scaling_d(&y_d);
   }

   if( honor_original_bounds_ && bound_relax_factor_ > 0.0 && (Px_L_->NCols() > 0 || Px_U_->NCols() > 0) )
   {
      // Make sure the user specified bounds are satisfied
      SmartPtr<Vector> tmp;
      SmartPtr<Vector> un_x = unscaled_x->MakeNewCopy();
      if( Px_L_->NCols() > 0 )
      {
         DBG_ASSERT(IsValid(orig_x_L_));
         tmp = orig_x_L_->MakeNewCopy();
         Px_L_->TransMultVector(1., *un_x, 0., *tmp);
         Px_L_->MultVector(-1., *tmp, 1., *un_x);
         tmp->ElementWiseMax(*orig_x_L_);
         Px_L_->MultVector(1., *tmp, 1., *un_x);
      }
      if( Px_U_->NCols() > 0 )
      {
         DBG_ASSERT(IsValid(orig_x_U_));
         tmp = orig_x_U_->MakeNewCopy();
         Px_U_->TransMultVector(1., *un_x, 0., *tmp);
         Px_U_->MultVector(-1., *tmp, 1., *un_x);
         tmp->ElementWiseMin(*orig_x_U_);
         Px_U_->MultVector(1., *tmp, 1., *un_x);
      }
      unscaled_x = ConstPtr(un_x);
   }

   unscaled_x->Print(*jnlst_, J_VECTOR, J_SOLUTION, "final x unscaled");
   unscaled_y_c->Print(*jnlst_, J_VECTOR, J_SOLUTION, "final y_c unscaled");
   unscaled_y_d->Print(*jnlst_, J_VECTOR, J_SOLUTION, "final y_d unscaled");
   unscaled_z_L->Print(*jnlst_, J_VECTOR, J_SOLUTION, "final z_L unscaled");
   unscaled_z_U->Print(*jnlst_, J_VECTOR, J_SOLUTION, "final z_U unscaled");

   nlp_->FinalizeSolution(status, *unscaled_x, *unscaled_z_L, *unscaled_z_U, *unscaled_c, *unscaled_d, *unscaled_y_c,
                          *unscaled_y_d, unscaled_obj, ip_data, ip_cq);
}

bool OrigIpoptNLP::IntermediateCallBack(
   AlgorithmMode                       mode,
   Index                               iter,
   Number                              obj_value,
   Number                              inf_pr,
   Number                              inf_du,
   Number                              mu,
   Number                              d_norm,
   Number                              regularization_size,
   Number                              alpha_du,
   Number                              alpha_pr,
   Index                               ls_trials,
   SmartPtr<const IpoptData>           ip_data,
   SmartPtr<IpoptCalculatedQuantities> ip_cq
)
{
   return nlp_->IntermediateCallBack(mode, iter, obj_value, inf_pr, inf_du, mu, d_norm, regularization_size, alpha_du,
                                     alpha_pr, ls_trials, GetRawPtr(ip_data), GetRawPtr(ip_cq));
}

void OrigIpoptNLP::AdjustVariableBounds(
   const Vector& new_x_L,
   const Vector& new_x_U,
   const Vector& new_d_L,
   const Vector& new_d_U
)
{
   x_L_ = new_x_L.MakeNewCopy();
   x_U_ = new_x_U.MakeNewCopy();
   d_L_ = new_d_L.MakeNewCopy();
   d_U_ = new_d_U.MakeNewCopy();
}

SmartPtr<const Vector> OrigIpoptNLP::get_unscaled_x(
   const Vector& x
)
{
   SmartPtr<const Vector> ret;
   if( !unscaled_x_cache_.GetCachedResult1Dep(ret, &x) )
   {
      ret = NLP_scaling()->unapply_vector_scaling_x(&x);
      unscaled_x_cache_.AddCachedResult1Dep(ret, &x);
   }
   return ret;
}

} // namespace Ipopt
